#include <iostream>
#include <string>
#include <sstream>
#include <boost/asio.hpp>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"

// 消息类型
#include "serial_tansport/imu.h"
#include "serial_tansport/move.h"
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>

using namespace std;
using namespace boost::asio;

///////// 通讯协议 /////////
const unsigned char ender[1] = {0x0a};        // 数据尾
const unsigned char header[2] = {0x55, 0xaa}; // 数据头

// 数据体-4字节
typedef union
{
    float data;
    unsigned char data8[4];
} data_4;

// 串口有关对象
boost::asio::io_service iosev; // io_service实例
boost::system::error_code err; // error_code实例

class mySerial
{
public:
    mySerial(string port_name, int Baudrate);
    boost::asio::serial_port sp = boost::asio::serial_port(iosev); // 串口对象
    int Baudrate;                                                  // 波特率
    string port_name;                                              // 串口名字
    bool init(string port_name, int Baudrate);                     // 初始化
};

mySerial::mySerial(string port_name, int Baudrate)
{
    bool flag = init(port_name, Baudrate);
    if (flag)
        cout << "mySerial init successfully" << endl;
    else
        cout << "mySerial fails to init " << endl;
}
bool mySerial::init(string port_name, int Baudrate)
{
    mySerial::Baudrate = Baudrate;
    mySerial::port_name = port_name;
    try
    {
        sp.open(port_name);
        sp.set_option(serial_port::baud_rate(Baudrate));                           // 设置波特率
        sp.set_option(serial_port::flow_control(serial_port::flow_control::none)); // 流量控制
        sp.set_option(serial_port::parity(serial_port::parity::none));             // 奇偶校验
        sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));        // 停止位
        sp.set_option(serial_port::character_size(8));                             // 字符大小
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what() << '\n';
        return 0;
    }

    return 1;
}

// 运动控制信息的发送函数
void moveControl(float v, float a, unsigned char ctrlFlag, mySerial *s)
{
    unsigned char buf[13] = {0}; // 缓冲器
    int length = 0;

    data_4 velocity, angle;
    velocity.data = v;
    angle.data = a;

    // 设置消息头
    for (int i = 0; i < 2; i++)
        buf[i] = header[i]; // buf[0]  buf[1]

    // 设置机器人左右轮速度
    length = 9;
    buf[2] = length; // buf[2]
    for (int i = 0; i < 4; i++)
    {
        buf[i + 3] = velocity.data8[i]; // buf[3] buf[4] buf[5] buf[6]
        buf[i + 7] = angle.data8[i];    // buf[7] buf[8] buf[9] buf[10]
    }
    // 预留控制指令
    buf[2 + length] = ctrlFlag; // buf[11]

    // 设置校验值、消息尾
    buf[3 + length] = ender[0]; // buf[12]

    // 通过串口下发数据
    boost::asio::write(s->sp, boost::asio::buffer(buf));
}

// 接受stm32的信息
// void readSerial(mySerial *s)
// {
//     unsigned char buf[13] = {0}; // 缓冲器
//     int length = 0;

//     data_4 data1, data2, data3;
//     velocity.data = v;
//     angle.data = a;

//     // 设置消息头
//     for (int i = 0; i < 2; i++)
//         buf[i] = header[i]; // buf[0]  buf[1]

//     // 设置机器人左右轮速度
//     length = 9;
//     buf[2] = length; // buf[2]
//     for (int i = 0; i < 4; i++)
//     {
//         buf[i + 3] = velocity.data8[i]; // buf[3] buf[4] buf[5] buf[6]
//         buf[i + 7] = angle.data8[i];    // buf[7] buf[8] buf[9] buf[10]
//     }
//     // 预留控制指令
//     buf[2 + length] = ctrlFlag; // buf[11]

//     // 设置校验值、消息尾
//     buf[3 + length] = ender[0]; // buf[12]

//     // 通过串口下发数据
//     boost::asio::write(s->sp, boost::asio::buffer(buf));
// }
